package tests;

import com.neuronrobotics.sdk.dyio.DyIO;
import com.neuronrobotics.sdk.dyio.peripherals.AnalogInputChannel;
import com.neuronrobotics.sdk.dyio.peripherals.ServoChannel;
import com.neuronrobotics.sdk.ui.ConnectionDialog;

public class Wall {
	public static void main(String[] args) throws InterruptedException {
		DyIO dyio=new DyIO();
		if (!ConnectionDialog.getBowlerDevice(dyio)){
			System.exit(1);
		}
		AnalogInputChannel ir_right = new AnalogInputChannel (dyio.getChannel(10)); //  right infrared 10
		AnalogInputChannel ir_left = new AnalogInputChannel (dyio.getChannel(11)); // left infrared port 11
		AnalogInputChannel us_left = new AnalogInputChannel (dyio.getChannel(9)); // left ultra sonic port 9
		AnalogInputChannel us_right = new AnalogInputChannel (dyio.getChannel(8)); // right ultra sonic port 8
		ServoChannel srv_left = new ServoChannel (dyio.getChannel(12)); //left wheel port 12
		ServoChannel srv_right = new ServoChannel (dyio.getChannel(13)); // right wheel port 13

		// I think floats and doubles are too accurate i want something to roughly round to the same value
		/*
		int A = (int) (60/((ir_left.getVoltage()) + (6/5)));  //Alinear = 60./((6/5)+A); 
		int B = (int) (60/((ir_right.getVoltage()) + (6/5)));  //Blinear = 60./((6/5)+B);
		int C = (int)  (4 + (100*(us_left.getVoltage())));  //Clinear = 4 + (100*C);
		int D = (int)  (3.725 + (100*(us_right.getVoltage()))); //Dlinear = 3.725 + (100*D);
		*/
	
while(true)
	{
	float A = (float) (60/((ir_left.getVoltage()) + (6/5)));  //Alinear = 60./((6/5)+A); 
	float B = (float) (60/((ir_right.getVoltage()) + (6/5)));  //Blinear = 60./((6/5)+B);
	float C = (float)  (4 + (100*(us_left.getVoltage())));  //Clinear = 4 + (100*C);
	float D = (float)  (3.75 + (100*(us_right.getVoltage()))); //Dlinear = 3.725 + (100*D);
	
	if ((A <= 20)||	(B <= 20)) //|| (C <=20) || (D <= 20))
	{	
		srv_left.SetPosition(128, 1);
		srv_right.SetPosition(128, 1);
		System.out.println("what side is greater?");
		//Thread.sleep(3000);
		if ((B > A)) //|| (D > C)) // if right side is greater than left side
		{
			System.out.println("right");
			srv_left.SetPosition(128,1 );
			srv_right.SetPosition(200, 10);
			Thread.sleep(100);
				
			if((B==A))// || (D == C)) // Maybe this will work? I want it to check this statement ASAP
			{
			while(true)
				{
			srv_left.SetPosition(128, 1);
			srv_right.SetPosition(128, 1);
			System.out.println( A +"	" + B + "	" + C + "	" + D);
			System.out.println("stop");
			Thread.sleep(3000);
				}
			}
//		else

		}
	}
	else
		{
		srv_left.SetPosition(75, 4);
		srv_right.SetPosition(180, 4);	
		Thread.sleep(1000);
		System.out.println( A +"	" + B + "	" + C + "	" + D);
		System.out.println( ir_left.getVoltage() +"	" + ir_right.getVoltage() + "	" + us_left.getVoltage() + "	" + us_right.getVoltage());
		srv_left.SetPosition(128, 1);
		srv_right.SetPosition(128, 1);
		Thread.sleep(1000);
		}

	}
}
}	


/*	
else if ((B > A) || (D > C))// if left side is greater than right side
	{
		if((B==A) || (D == C))
		{
			srv_left.SetPosition(128, 10);
			srv_right.SetPosition(128, 1);
			System.out.println( A +"	" + B + "	" + C + "	" + D);
			Thread.sleep(3000);
		}
		else
		{
			srv_left.SetPosition(128, 1);
			srv_right.SetPosition(200, 10);
			Thread.sleep(100);
		}
	}
}
*/